# Vision Interface for soul planner
# By: Camiel Verschoor

import threading
import vision
import longDistanceTracker as ball
import locateGoal as goal
import time
from naoqi import ALProxy
import lineFinder
import math

class goalFilter(threading.Thread):
    active = False
    color = None
    image = None
    head = None
    info = None
    tempGoalPost = None
    
    def __init__(self, name, color):
        threading.Thread.__init__(self)
        self.name = name
        self.color = color

    def run(self):
        self.active = True
        # Check for No input
        if(self.image == None or self.head[0] == None or self.head[1] == None):
            self.image = None
            self.head = None
        else:
            self.info = goal.run(self.image, self.color, self.head)
        self.active = False

    # Checks if the filter is running
    def isActive(self):
        return self.active

    # Gets the info
    def get(self):
        info = self.info
        self.info = None
        self.image = None
        return info

    # Puts image
    def put(self, image, head):
        self.image = image
        self.head = head
        

# Vision ID
visionID = vision.subscribe()
# METHODS FOR VISION

def findGoal():
    now = time.time()
    # Take snapshot and measure head angles
    (image, headinfo) = vision.snapShot(visionID)
    # Filter the snapshots and return the angle and the color
    (_, head) = headinfo
    info = filterGoal(image, head)
    print time.time() - now
    return info

def findBall():
    # Take snapshot and measure head angles
    (image, head) = vision.snapShot(visionID)
    # Filter the snapshots and return the angle and the color
    ballLoc = ball.run(image, head)
    if ballLoc:
        (x,y) = ballLoc
        if x < 0 or x > 6 or y < -6 or y > 6:
            ballLoc = None
    return ballLoc

# FUNCTION FOR GOAL
def filterGoal(image, head): #, tempGoalPostBlue, tempGoalPostYellow):
    # Create threads
    blueGoal = goalFilter("blueGoalFilter", "blue")
    yellowGoal = goalFilter("yellowGoalFilter", "yellow")
    # Transfer the info to the threads
    blueGoal.put(image, head)
    yellowGoal.put(image, head)
    # Run the threads
    blueGoal.start()
    yellowGoal.start()
    # Join threads
    while blueGoal.isActive() or yellowGoal.isActive():
        pass
    # Retrieve information
    blue = blueGoal.get()
    yellow = yellowGoal.get()
    # Check
    if blue:
        return ('Blue', blue)
    elif yellow:
        return ('Yellow', yellow)
        
def pK():
    (image, headinfo) = vision.snapShot(visionID)
    lines, yaw = lineFinder.pKeep((image, headinfo))
    print 'LINES', lines
    if( len(lines) > 1):  
        longestSoFar = lines[0]
        for i in (range(len(lines)-1)):
            longest = getLongestLine(lines[i],lines[i+1])
            longestSoFar = getLongestLine(longest, longestSoFar)
        turn = parralleline(longestSoFar, yaw)
        print "TURNING ANGLE:", turn
        return turn, longestSoFar
    elif(len(lines) == 0):
        print 'no lines found'
        return 0, []
    else:   
        turn= parralleline(lines[0], yaw)
        print "TURNING ANGLE:", turn 
        return turn, lines[0]

def getLongestLine(line1, line2):
    length1 = lineLength(line1)
    length2 = lineLength(line2)
    if length1 > length2:
        return line1
    return line2

'''
Returns the length of a line.
'''
def lineLength(line):
    ((x11,y11), (x12,y12)) = line
    return math.sqrt(math.pow(x11-x12,2)+math.pow(y11-y12,2))
        
        
        
def parralleline(line, yaw):
    #(x,y,z, roll, pitch, yaw) = cam
    angleL = angleLine(line)
    # if the line is turned to the right then the nao has
    #     to turn left...
    # adjust the angle according to the yaw of the camera
    #  so that the body (and not the head) is parrallel
    #  with the line
    angle = -(angleL -yaw) 
    #motion.walkTo(0, 0, angle)
    return angle
    
def angleLine((p1, p2)):
    (x1, y1) = p1
    (x2, y2) = p2
    return math.atan2(y2-y1, x2-x1)
    
def findLine():
    (image, headinfo) = vision.snapShot(visionID)
    lines, yaw = lineFinder.pKeep((image, headinfo))
    if(len(lines) >= 1):
        return True
    else:
        return False
    